﻿#define OLD_SCHOOL_BALL_SOCKET

using System;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics
{
#if OLD_SCHOOL_BALL_SOCKET
    /// <summary>
    /// Constrains a relative position on one <see cref="RigidBody"/> to be fixed to 
    /// a relative position on another <see cref="RigidBody"/>.
    /// </summary>
    public class PointToPointConstraint : Constraint
    {
        private RigidBody rigidBody0;
        private RigidBody rigidBody1;

        private Vector3 position0, position1;
        private Vector3 correctionVector;

        /// <summary>
        /// Gets or sets the first <see cref="RigidBody"/> instance's relative position to the shared point.
        /// </summary>
        public Vector3 RelativePosition0;

        /// <summary>
        /// Gets or sets the second <see cref="RigidBody"/> instance's relative position to the shared point.
        /// </summary>
        public Vector3 RelativePosition1;

        /// <summary>
        /// Initializes a new instances of the <see cref="PointToPointConstraint"/> class.
        /// </summary>
        public PointToPointConstraint() : base() 
        {	
            // Default some properties.
            this.Tolerance = 0.05f;
            this.Timescale = 1.0f;
        }

        /// <summary>
        /// Gets or sets the first <see cref="RigidBody"/> involved in the <see cref="PointToPointConstraint"/>.
        /// </summary>
        public RigidBody RigidBody0
        {
            get { return this.rigidBody0; }
            set
            {
                // Make sure rigid body-to-constaint associations are still valid.
                if ((this.Flags & ConstraintFlags.Registered) == ConstraintFlags.Registered)
                {
                    if (this.rigidBody0 != null)
                    {
                        this.rigidBody0.Constraints.Items.Remove(this);
                    }
                    if (value != null)
                    {
                        value.Constraints.Items.Add(this);
                    }
                }

                // Hold onto value.
                this.rigidBody0 = value;
            }
        }

        /// <summary>
        /// Gets or sets the second <see cref="RigidBody"/> involved in the <see cref="PointToPointConstraint"/>.
        /// </summary>
        public RigidBody RigidBody1
        {
            get { return this.rigidBody1; }
            set
            {
                // Make sure rigid body-to-constaint associations are still valid.
                if ((this.Flags & ConstraintFlags.Registered) == ConstraintFlags.Registered)
                {
                    if (this.rigidBody1 != null)
                    {
                        this.rigidBody1.Constraints.Items.Remove(this);
                    }
                    if (value != null)
                    {
                        value.Constraints.Items.Add(this);
                    }
                }

                // Hold onto value.
                this.rigidBody1 = value;
            }
        }

        /// <summary>
        /// Gets or sets the tolerance of the <see cref="PointToPointConstraint"/>.
        /// </summary>
        /// <remarks>
        /// The <see cref="PointToPointConstraint"/> is considered unsatisified if the distance between the 
        /// <see cref="PointToPointConstraint.RelativePosition0"/> and <see cref="PointToPointConstraint.RelativePosition1"/>
        /// is greater than this value.
        /// </remarks>
        public float Tolerance { get; set; }

        /// <summary>
        /// Gets or sets the time scale used to satisfy the <see cref="PointToPointConstraint"/>.
        /// </summary>
        /// <remarks>
        /// An unsatisfied <see cref="PointToPointConstraint"/> will attempt to satisfy during the time specified with this value.
        /// </remarks>
        public float Timescale { get; set; }

        #region Constraint Members

        internal protected override void OnAdded()
        {
            // Call inherited method.
            base.OnAdded();

            // Associate the constraint with the rigid bodies.
            if (this.rigidBody0 != null)
            {
                this.rigidBody0.Constraints.Items.Add(this);
            }
            if (this.rigidBody1 != null)
            {
                this.rigidBody1.Constraints.Items.Add(this);
            }
        }

        internal protected override void OnRemoved()
        {
            // Call inherited method.
            base.OnRemoved();

            // Disassociated the constraint with the rigid bodies.
            if (this.rigidBody0 != null)
            {
                this.rigidBody0.Constraints.Items.Remove(this);
            }
            if (this.rigidBody1 != null)
            {
                this.rigidBody1.Constraints.Items.Remove(this);
            }
        }
		
        internal protected override void PreStep(float elapsed)
        {
#if INLINE
            // Get the oriented relative position of the fixed point to each rigid body.
            this.position0.X = this.RelativePosition0.X * this.rigidBody0.Orientation.M11 + this.RelativePosition0.Y * this.rigidBody0.Orientation.M21 + this.RelativePosition0.Z * this.rigidBody0.Orientation.M31;
            this.position0.Y = this.RelativePosition0.X * this.rigidBody0.Orientation.M12 + this.RelativePosition0.Y * this.rigidBody0.Orientation.M22 + this.RelativePosition0.Z * this.rigidBody0.Orientation.M32;
            this.position0.Z = this.RelativePosition0.X * this.rigidBody0.Orientation.M13 + this.RelativePosition0.Y * this.rigidBody0.Orientation.M23 + this.RelativePosition0.Z * this.rigidBody0.Orientation.M33;

            this.position1.X = this.RelativePosition1.X * this.rigidBody1.Orientation.M11 + this.RelativePosition1.Y * this.rigidBody1.Orientation.M21 + this.RelativePosition1.Z * this.rigidBody1.Orientation.M31;
            this.position1.Y = this.RelativePosition1.X * this.rigidBody1.Orientation.M12 + this.RelativePosition1.Y * this.rigidBody1.Orientation.M22 + this.RelativePosition1.Z * this.rigidBody1.Orientation.M32;
            this.position1.Z = this.RelativePosition1.X * this.rigidBody1.Orientation.M13 + this.RelativePosition1.Y * this.rigidBody1.Orientation.M23 + this.RelativePosition1.Z * this.rigidBody1.Orientation.M33;

            // Get the world positions of the relative positions to the fixed point of each rigid body.
            float x0 = this.rigidBody0.Position.X + this.position0.X;
            float y0 = this.rigidBody0.Position.Y + this.position0.Y;
            float z0 = this.rigidBody0.Position.Z + this.position0.Z;

            float x1 = this.rigidBody1.Position.X + this.position1.X;
            float y1 = this.rigidBody1.Position.Y + this.position1.Y;
            float z1 = this.rigidBody1.Position.Z + this.position1.Z;

            // Get the vector between the two world positions.
            Vector3 difference = new Vector3()
            {
                X = x0 - x1,
                Y = y0 - y1,
                Z = z0 - z1
            };
			
            // Determine the correctional vector necessary to move the rigid bodies back to satisfy this constraint.
            float distance = difference.Length();
            if (distance > this.Tolerance)
            {
                float temp = (distance - this.Tolerance) / (distance * Math.Max(this.Timescale, elapsed));
                this.correctionVector.X = difference.X * temp;
                this.correctionVector.Y = difference.Y * temp;
                this.correctionVector.Z = difference.Z * temp;

                this.satisfied = false;
            }
            else
            {
                this.correctionVector = Vector3.Zero;
                this.satisfied = true;
            }
#elif REFERENCE
            // Get the oriented relative position of the fixed point to each rigid body.
            Vector3.TransformNormal(ref this.RelativePosition0, ref this.rigidBody0.Orientation, out this.position0);
            Vector3.TransformNormal(ref this.RelativePosition1, ref this.rigidBody1.Orientation, out this.position1);

            // Get the world positions of the relative positions to the fixed point of each rigid body.
            Vector3 worldPosition0, worldPosition1;
            Vector3.Add(ref this.rigidBody0.Position, ref this.position0, out worldPosition0);
            Vector3.Add(ref this.rigidBody1.Position, ref this.position1, out worldPosition1);

            // Get the vector between the two world positions.
            Vector3 difference;
            Vector3.Subtract(ref worldPosition0, ref worldPosition1, out difference);

            // Determine the correctional vector necessary to move the rigid bodies back to satisfy this constraint.
            float distance = difference.Length();
            if (distance > this.Tolerance)
            {
                Vector3.Multiply(ref difference, (distance - this.Tolerance) / (distance * Math.Max(this.Timescale, elapsed)), out this.correctionVector);
                this.satisfied = false;
            }
            else
            {
                this.correctionVector = Vector3.Zero;
                this.satisfied = true;
            }
#else
            // Get the oriented relative position of the fixed point to each rigid body.
            this.position0 = Vector3.TransformNormal(this.RelativePosition0, this.rigidBody0.Orientation);
            this.position1 = Vector3.TransformNormal(this.RelativePosition1, this.rigidBody1.Orientation);
			
            // Get the world positions of the relative positions to the fixed point of each rigid body.
            Vector3 worldPosition0 = this.rigidBody0.Position + this.position0;
            Vector3 worldPosition1 = this.rigidBody1.Position + this.position1;
						
            // Determine the vector between the two world positions.
            Vector3 difference = worldPosition0 - worldPosition1;

            // Determine the correctional vector necessary to move the rigid bodies back to satisfy this constraint.
            float distance = difference.Length();
            if (distance > this.Tolerance)
            {
                this.correctionVector = difference * ((distance - this.Tolerance) / (distance * Math.Max(this.Timescale, elapsed)));
                this.satisfied = false;
            }
            else
            {
                this.correctionVector = Vector3.Zero;
                this.satisfied = true;
            }
#endif
        }

        internal protected override bool Solve(float elapsed)
        {
            // Assume the constraint has not been broken.
            this.satisfied = true;

            // Exit if both rigid bodies are disabled.
            if ((this.rigidBody0.Flags & this.rigidBody1.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.Disabled)
            {
                return false;
            }
#if INLINE
            // Get the relative velocity at the fixed point of the first rigid body.
			float x0, y0, z0;
            x0 = y0 = z0 = 0.0f;
            if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
		        x0 = this.rigidBody0.Velocity.X + ((this.rigidBody0.Rotation.Y * this.position0.Z) - (this.rigidBody0.Rotation.Z * this.position0.Y));
	            y0 = this.rigidBody0.Velocity.Y + ((this.rigidBody0.Rotation.Z * this.position0.X) - (this.rigidBody0.Rotation.X * this.position0.Z));
				z0 = this.rigidBody0.Velocity.Z + ((this.rigidBody0.Rotation.X * this.position0.Y) - (this.rigidBody0.Rotation.Y * this.position0.X));
			}

			// Get the relative velocity at the fixed point of the second rigid body.
            float x1, y1, z1;
            x1 = y1 = z1 = 0.0f;
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
                x1 = this.rigidBody1.Velocity.X + ((this.rigidBody1.Rotation.Y * this.position1.Z) - (this.rigidBody1.Rotation.Z * this.position1.Y));
                y1 = this.rigidBody1.Velocity.Y + ((this.rigidBody1.Rotation.Z * this.position1.X) - (this.rigidBody1.Rotation.X * this.position1.Z));
                z1 = this.rigidBody1.Velocity.Z + ((this.rigidBody1.Rotation.X * this.position1.Y) - (this.rigidBody1.Rotation.Y * this.position1.X));
            }

            // Get the combined relative velocity of the two rigid bodies, including the correction vector.
            Vector3 relativeVelocity = new Vector3()
            {
                X = this.correctionVector.X + x0 - x1,
                Y = this.correctionVector.Y + y0 - y1,
                Z = this.correctionVector.Z + z0 - z1
            };
#elif REFERENCE
            // Get the relative velocity at the fixed point of the first rigid body.
            Vector3 relativeVelocity0 = Vector3.Zero;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				Vector3.Cross(ref this.rigidBody0.Rotation, ref this.position0, out relativeVelocity0);
				Vector3.Add(ref relativeVelocity0, ref this.rigidBody0.Velocity, out relativeVelocity0);
			}

            // Get the relative velocity at the fixed point of the second rigid body.
            Vector3 relativeVelocity1 = Vector3.Zero;
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
                Vector3.Cross(ref this.rigidBody1.Rotation, ref this.position1, out relativeVelocity1);
                Vector3.Add(ref relativeVelocity1, ref this.rigidBody1.Velocity, out relativeVelocity1);
            }

            // Get the combined relative velocity of the two rigid bodies, including the correctional vector.
            Vector3 relativeVelocity;
            Vector3.Add(ref this.correctionVector, ref relativeVelocity0, out relativeVelocity);
            Vector3.Subtract(ref relativeVelocity, ref relativeVelocity1, out relativeVelocity);
#else
			// Get the relative velocity at the fixed point of the each rigid body.
			Vector3 relativeVelocity0 = Vector3.Zero;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				relativeVelocity0 = this.rigidBody0.Velocity + Vector3.Cross(this.rigidBody0.Rotation, this.position0);
			}
            Vector3 relativeVelocity1 = Vector3.Zero;
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
                relativeVelocity1 = this.rigidBody1.Velocity + Vector3.Cross(this.rigidBody1.Rotation, this.position1);
            }

            // Get the combined relative velocity of the two rigid bodies, including the correction vector.
            Vector3 relativeVelocity = this.correctionVector + relativeVelocity0 - relativeVelocity1;
#endif
            // Determine the velocity magnitude in the direction of the combined relative velocity.
            float normalVelocity = relativeVelocity.Length();
			
            // TODO : Remove magic number.
            // No further processing is necessary if the velocity is too small.
            if (normalVelocity <= 0.0009f)
            {
                return false;
            }
#if INLINE
            // Unitize the relative velocity.  This gives the direction the impulse needs to be applied.
            float temp0 = 1.0f / normalVelocity;
            Vector3 normal;
            normal.X = relativeVelocity.X * temp0;
            normal.Y = relativeVelocity.Y * temp0;
            normal.Z = relativeVelocity.Z * temp0;

            // Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
            // the first rigid body.
			float velocityChange = 0.0f;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				float x = this.position0.Y * normal.Z - this.position0.Z * normal.Y;
				float y = this.position0.Z * normal.X - this.position0.X * normal.Z;
				float z = this.position0.X * normal.Y - this.position0.Y * normal.X;

				float tx = x * this.rigidBody0.inverseInertiaTensorWorld.M11 + y * this.rigidBody0.inverseInertiaTensorWorld.M21 + z * this.rigidBody0.inverseInertiaTensorWorld.M31;
				float ty = x * this.rigidBody0.inverseInertiaTensorWorld.M12 + y * this.rigidBody0.inverseInertiaTensorWorld.M22 + z * this.rigidBody0.inverseInertiaTensorWorld.M32;
				float tz = x * this.rigidBody0.inverseInertiaTensorWorld.M13 + y * this.rigidBody0.inverseInertiaTensorWorld.M23 + z * this.rigidBody0.inverseInertiaTensorWorld.M33;

				x = ty * this.position0.Z - tz * this.position0.Y;
				y = tz * this.position0.X - tx * this.position0.Z;
				z = tx * this.position0.Y - ty * this.position0.X;

				velocityChange = this.rigidBody0.inverseMass + (normal.X * x + normal.Y * y + normal.Z * z);
			}

            // Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
            // the second rigid body.  Combine with the first rigid body.
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
				float x = this.position1.Y * normal.Z - this.position1.Z * normal.Y;
				float y = this.position1.Z * normal.X - this.position1.X * normal.Z;
				float z = this.position1.X * normal.Y - this.position1.Y * normal.X;

				float tx = x * this.rigidBody1.inverseInertiaTensorWorld.M11 + y * this.rigidBody1.inverseInertiaTensorWorld.M21 + z * this.rigidBody1.inverseInertiaTensorWorld.M31;
				float ty = x * this.rigidBody1.inverseInertiaTensorWorld.M12 + y * this.rigidBody1.inverseInertiaTensorWorld.M22 + z * this.rigidBody1.inverseInertiaTensorWorld.M32;
				float tz = x * this.rigidBody1.inverseInertiaTensorWorld.M13 + y * this.rigidBody1.inverseInertiaTensorWorld.M23 + z * this.rigidBody1.inverseInertiaTensorWorld.M33;

                x = ty * this.position1.Z - tz * this.position1.Y;
                y = tz * this.position1.X - tx * this.position1.Z;
                z = tx * this.position1.Y - ty * this.position1.X;

                velocityChange += this.rigidBody1.inverseMass + (normal.X * x + normal.Y * y + normal.Z * z);
            }
#elif REFERENCE
            // Unitize the relative velocity.  This gives the direction the impulse needs to be applied.
            Vector3 normal;
            Vector3.Divide(ref relativeVelocity, normalVelocity, out normal);

            // Determine the magnitude of an impulse necessary ro remove all relative velocity at the relative fixed point of
            // the first rigid body. 
			float velocityChange = 0.0f;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				Vector3 temp;
				Vector3.Cross(ref this.position0, ref normal, out temp);
				Vector3.TransformNormal(ref temp, ref this.rigidBody0.inverseInertiaTensorWorld, out temp);
				Vector3.Cross(ref temp, ref this.position0, out temp);
				velocityChange = this.rigidBody0.inverseMass + Vector3Extension.Dot(ref normal, ref temp);
			}

            // Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed point of
            // the second rigid body.  Apply it to the first.
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
                Vector3.Cross(ref this.position1, ref normal, out temp);
                Vector3.TransformNormal(ref temp, ref this.rigidBody1.inverseInertiaTensorWorld, out temp);
                Vector3.Cross(ref temp, ref this.position1, out temp);
                velocityChange += this.rigidBody1.inverseMass + Vector3Extension.Dot(ref normal, ref temp);
            }
#else
			// Unitize the relative velocity.  This gives the direction the impulse needs to be applied.
            Vector3 normal = relativeVelocity / normalVelocity;
			
            // Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
            // each rigid body and combine them.
			float velocityChange = 0.0f;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				velocityChange = this.rigidBody0.inverseMass + Vector3.Dot(normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(this.position0, normal), this.rigidBody0.inverseInertiaTensorWorld), this.position0));
			}
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
                velocityChange += this.rigidBody1.inverseMass + Vector3.Dot(normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(this.position1, normal), this.rigidBody1.inverseInertiaTensorWorld), this.position1));
            }
#endif
            if (velocityChange > MathExtension.Epsilon)
            {
                // Determine the vector impulse necessary to move each rigid body closer to making this
                // constraint satisfied.
#if INLINE
                float temp1 = -normalVelocity / velocityChange;
                Vector3 normalImpulse = new Vector3()
                {
                    X = normal.X * temp1,
                    Y = normal.Y * temp1,
                    Z = normal.Z * temp1
                };
#elif REFERENCE
                Vector3 normalImpulse;
                Vector3.Multiply(ref normal, -normalVelocity / velocityChange, out normalImpulse);
#else
                Vector3 normalImpulse = normal * (-normalVelocity / velocityChange);
#endif
                // Apply the impulse to each rigid body.
                this.rigidBody0.ApplyOffsetForceImpulse(ref normalImpulse, ref this.position0);
                this.rigidBody1.ApplyReverseOffsetForceImpulse(ref normalImpulse, ref this.position1);
				
                // Reset all collisions and constraints - a velocity change occurred.
                this.rigidBody0.CollisionGroup.Collisions.Reset();
                this.rigidBody0.Constraints.Reset();
                this.rigidBody1.CollisionGroup.Collisions.Reset();
                this.rigidBody1.Constraints.Reset();

                // Keep this constraint satisfied.
                this.satisfied = true;

                // Some more processing may be necessary.
                return true;
            }

            // The constraint does not need further processing.
            return false;
        }

        #endregion
    }
#else
	// TODO : For some reason, the XBox 360 doesn't like this version of the p-2-p.  Maybe due to iterations and not handling it correctly (i.e., elapsed time) since physics is slower on the XBOX.
	/// <summary>
	/// Constrains a relative position on one <see cref="RigidBody"/> to be fixed to 
	/// a relative position on another <see cref="RigidBody"/>.
	/// </summary>
	public class PointToPointConstraint : Constraint
	{
		private RigidBody rigidBody0;
		private RigidBody rigidBody1;

		private Vector3 position0, position1;
		private Vector3 normal;

		private float velocityChange;
		private float depth;

		/// <summary>
		/// Gets or sets the first <see cref="RigidBody"/> instance's relative position to the shared point.
		/// </summary>
		public Vector3 RelativePosition0;

		/// <summary>
		/// Gets or sets the second <see cref="RigidBody"/> instance's relative position to the shared point.
		/// </summary>
		public Vector3 RelativePosition1;

		/// <summary>
		/// Initializes a new instances of the <see cref="PointToPointConstraint"/> class.
		/// </summary>
		public PointToPointConstraint() : base()
		{
			// Default some properties.
			this.Tolerance = 0.05f;
			this.Timescale = 1.0f;
		}

		/// <summary>
		/// Gets or sets the first <see cref="RigidBody"/> involved in the <see cref="PointToPointConstraint"/>.
		/// </summary>
		public RigidBody RigidBody0
		{
			get { return this.rigidBody0; }
			set
			{
				// Make sure rigid body-to-constaint associations are still valid.
				if ((this.Flags & ConstraintFlags.Registered) == ConstraintFlags.Registered)
				{
					if (this.rigidBody0 != null)
					{
						this.rigidBody0.Constraints.Items.Remove(this);
					}
					if (value != null)
					{
						value.Constraints.Items.Add(this);
					}
				}

				// Hold onto value.
				this.rigidBody0 = value;
			}
		}

		/// <summary>
		/// Gets or sets the second <see cref="RigidBody"/> involved in the <see cref="PointToPointConstraint"/>.
		/// </summary>
		public RigidBody RigidBody1
		{
			get { return this.rigidBody1; }
			set
			{
				// Make sure rigid body-to-constaint associations are still valid.
				if ((this.Flags & ConstraintFlags.Registered) == ConstraintFlags.Registered)
				{
					if (this.rigidBody1 != null)
					{
						this.rigidBody1.Constraints.Items.Remove(this);
					}
					if (value != null)
					{
						value.Constraints.Items.Add(this);
					}
				}

				// Hold onto value.
				this.rigidBody1 = value;
			}
		}

		/// <summary>
		/// Gets or sets the tolerance of the <see cref="PointToPointConstraint"/>.
		/// </summary>
		/// <remarks>
		/// The <see cref="PointToPointConstraint"/> is considered unsatisified if the distance between the 
		/// <see cref="PointToPointConstraint.RelativePosition0"/> and <see cref="PointToPointConstraint.RelativePosition1"/>
		/// is greater than this value.
		/// </remarks>
		public float Tolerance { get; set; }

		/// <summary>
		/// Gets or sets the time scale used to satisfy the <see cref="PointToPointConstraint"/>.
		/// </summary>
		/// <remarks>
		/// An unsatisfied <see cref="PointToPointConstraint"/> will attempt to satisfy during the time specified with this value.
		/// </remarks>
		public float Timescale { get; set; }

		#region Constraint Members

		internal protected override void OnAdded()
		{
			// Call inherited method.
			base.OnAdded();

			// Associate the constraint with the rigid bodies.
			if (this.rigidBody0 != null)
			{
				this.rigidBody0.Constraints.Items.Add(this);
			}
			if (this.rigidBody1 != null)
			{
				this.rigidBody1.Constraints.Items.Add(this);
			}
		}

		internal protected override void OnRemoved()
		{
			// Call inherited method.
			base.OnRemoved();

			// Disassociated the constraint with the rigid bodies.
			if (this.rigidBody0 != null)
			{
				this.rigidBody0.Constraints.Items.Remove(this);
			}
			if (this.rigidBody1 != null)
			{
				this.rigidBody1.Constraints.Items.Remove(this);
			}
		}

		internal protected override void PreStep(float elapsed)
		{
#if QUATERNION_ORIENTATION
			// Get the oriented relative position of the fixed point to each rigid body.
			this.position0 = Vector3.Transform(this.RelativePosition0, this.rigidBody0.Orientation);
			this.position1 = Vector3.Transform(this.RelativePosition1, this.rigidBody1.Orientation);

			// Get the world positions of the relative positions to the fixed point of each rigid body.
			Vector3 worldPosition0 = this.rigidBody0.Position + this.position0;
			Vector3 worldPosition1 = this.rigidBody1.Position + this.position1;

			// Get the vector between the two world positions.
			Vector3 difference = worldPosition0 - worldPosition1;
			if (difference.LengthSquared() > MathExtension.Epsilon * MathExtension.Epsilon)
			{
				// Unitize the differenct in position to get the correction
				// normal.
				this.normal = Vector3.Normalize(difference);

				// Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
				// each rigid body and combine them.
				this.velocityChange = 0.0f;
				if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					this.velocityChange = this.rigidBody0.inverseMass + Vector3.Dot(this.normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(this.position0, this.normal), this.rigidBody0.inverseInertiaTensorWorld), this.position0));
				}
				if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					this.velocityChange += this.rigidBody1.inverseMass + Vector3.Dot(this.normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(this.position1, this.normal), this.rigidBody1.inverseInertiaTensorWorld), this.position1));
				}

				// Get the error projected onto the normal.
				this.depth = -Vector3.Dot(this.normal, difference);

				this.satisfied = this.velocityChange <= MathExtension.Epsilon;
			}
			else
			{
				this.normal = Vector3.Zero;
				this.satisfied = true;
			}
#else
#if INLINE
			// Get the oriented relative position of the fixed point to each rigid body.
            this.position0.X = this.RelativePosition0.X * this.rigidBody0.Orientation.M11 + this.RelativePosition0.Y * this.rigidBody0.Orientation.M21 + this.RelativePosition0.Z * this.rigidBody0.Orientation.M31;
            this.position0.Y = this.RelativePosition0.X * this.rigidBody0.Orientation.M12 + this.RelativePosition0.Y * this.rigidBody0.Orientation.M22 + this.RelativePosition0.Z * this.rigidBody0.Orientation.M32;
            this.position0.Z = this.RelativePosition0.X * this.rigidBody0.Orientation.M13 + this.RelativePosition0.Y * this.rigidBody0.Orientation.M23 + this.RelativePosition0.Z * this.rigidBody0.Orientation.M33;

            this.position1.X = this.RelativePosition1.X * this.rigidBody1.Orientation.M11 + this.RelativePosition1.Y * this.rigidBody1.Orientation.M21 + this.RelativePosition1.Z * this.rigidBody1.Orientation.M31;
            this.position1.Y = this.RelativePosition1.X * this.rigidBody1.Orientation.M12 + this.RelativePosition1.Y * this.rigidBody1.Orientation.M22 + this.RelativePosition1.Z * this.rigidBody1.Orientation.M32;
            this.position1.Z = this.RelativePosition1.X * this.rigidBody1.Orientation.M13 + this.RelativePosition1.Y * this.rigidBody1.Orientation.M23 + this.RelativePosition1.Z * this.rigidBody1.Orientation.M33;

            // Get the world positions of the relative positions to the fixed point of each rigid body.
            float x0 = this.rigidBody0.Position.X + this.position0.X;
            float y0 = this.rigidBody0.Position.Y + this.position0.Y;
            float z0 = this.rigidBody0.Position.Z + this.position0.Z;

            float x1 = this.rigidBody1.Position.X + this.position1.X;
            float y1 = this.rigidBody1.Position.Y + this.position1.Y;
            float z1 = this.rigidBody1.Position.Z + this.position1.Z;

            // Get the vector between the two world positions.
            Vector3 difference = new Vector3()
            {
                X = x0 - x1,
                Y = y0 - y1,
                Z = z0 - z1
            };

			if (difference.LengthSquared() > MathExtension.Epsilon * MathExtension.Epsilon)
			{
				// Unitize the differenct in position to get the correction
				// normal.
				Vector3Extension.SafeNormalize(ref difference, out this.normal);

				// Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
				// the first rigid body.
				this.velocityChange = 0.0f;
				if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					float x = this.position0.Y * this.normal.Z - this.position0.Z * this.normal.Y;
					float y = this.position0.Z * this.normal.X - this.position0.X * this.normal.Z;
					float z = this.position0.X * this.normal.Y - this.position0.Y * this.normal.X;

					float tx = x * this.rigidBody0.inverseInertiaTensorWorld.M11 + y * this.rigidBody0.inverseInertiaTensorWorld.M21 + z * this.rigidBody0.inverseInertiaTensorWorld.M31;
					float ty = x * this.rigidBody0.inverseInertiaTensorWorld.M12 + y * this.rigidBody0.inverseInertiaTensorWorld.M22 + z * this.rigidBody0.inverseInertiaTensorWorld.M32;
					float tz = x * this.rigidBody0.inverseInertiaTensorWorld.M13 + y * this.rigidBody0.inverseInertiaTensorWorld.M23 + z * this.rigidBody0.inverseInertiaTensorWorld.M33;

					x = ty * this.position0.Z - tz * this.position0.Y;
					y = tz * this.position0.X - tx * this.position0.Z;
					z = tx * this.position0.Y - ty * this.position0.X;

					this.velocityChange = this.rigidBody0.inverseMass + (this.normal.X * x + this.normal.Y * y + this.normal.Z * z);
				}
				// Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
				// the second rigid body.  Combine with the first rigid body.
				if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					float x = this.position1.Y * this.normal.Z - this.position1.Z * this.normal.Y;
					float y = this.position1.Z * this.normal.X - this.position1.X * this.normal.Z;
					float z = this.position1.X * this.normal.Y - this.position1.Y * this.normal.X;

					float tx = x * this.rigidBody1.inverseInertiaTensorWorld.M11 + y * this.rigidBody1.inverseInertiaTensorWorld.M21 + z * this.rigidBody1.inverseInertiaTensorWorld.M31;
					float ty = x * this.rigidBody1.inverseInertiaTensorWorld.M12 + y * this.rigidBody1.inverseInertiaTensorWorld.M22 + z * this.rigidBody1.inverseInertiaTensorWorld.M32;
					float tz = x * this.rigidBody1.inverseInertiaTensorWorld.M13 + y * this.rigidBody1.inverseInertiaTensorWorld.M23 + z * this.rigidBody1.inverseInertiaTensorWorld.M33;

					x = ty * this.position1.Z - tz * this.position1.Y;
					y = tz * this.position1.X - tx * this.position1.Z;
					z = tx * this.position1.Y - ty * this.position1.X;

					this.velocityChange += this.rigidBody1.inverseMass + (this.normal.X * x + this.normal.Y * y + this.normal.Z * z);
				}

				// Get the error projected onto the normal.
				this.depth = -(this.normal.X * difference.X + this.normal.Y * difference.Y + this.normal.Z * difference.Z);

				this.satisfied = this.velocityChange <= MathExtension.Epsilon;
			}
			else
			{
				this.normal = Vector3.Zero;
				this.satisfied = true;
			}
#elif REFERENCE
			// Get the oriented relative position of the fixed point to each rigid body.
            Vector3.TransformNormal(ref this.RelativePosition0, ref this.rigidBody0.Orientation, out this.position0);
            Vector3.TransformNormal(ref this.RelativePosition1, ref this.rigidBody1.Orientation, out this.position1);

            // Get the world positions of the relative positions to the fixed point of each rigid body.
            Vector3 worldPosition0, worldPosition1;
            Vector3.Add(ref this.rigidBody0.Position, ref this.position0, out worldPosition0);
            Vector3.Add(ref this.rigidBody1.Position, ref this.position1, out worldPosition1);

            // Get the vector between the two world positions.
            Vector3 difference;
            Vector3.Subtract(ref worldPosition0, ref worldPosition1, out difference);

			if (difference.LengthSquared() > MathExtension.Epsilon * MathExtension.Epsilon)
			{
				// Unitize the differenct in position to get the correction
				// normal.
				Vector3Extension.SafeNormalize(ref difference, out this.normal);

				// Determine the magnitude of an impulse necessary ro remove all relative velocity at the relative fixed point of
				// the first rigid body.
				this.velocityChange = 0.0f;
				if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					Vector3 temp;
					Vector3.Cross(ref this.position0, ref this.normal, out temp);
					Vector3.TransformNormal(ref temp, ref this.rigidBody0.inverseInertiaTensorWorld, out temp);
					Vector3.Cross(ref temp, ref this.position0, out temp);
					this.velocityChange = this.rigidBody0.inverseMass + Vector3Extension.Dot(ref this.normal, ref temp);
				}
				// Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed point of
				// the second rigid body.  Apply it to the first.
				if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					Vector3 temp;
					Vector3.Cross(ref this.position1, ref this.normal, out temp);
					Vector3.TransformNormal(ref temp, ref this.rigidBody1.inverseInertiaTensorWorld, out temp);
					Vector3.Cross(ref temp, ref this.position1, out temp);
					this.velocityChange += this.rigidBody1.inverseMass + Vector3Extension.Dot(ref this.normal, ref temp);
				}

				// Get the error projected onto the normal.
				Vector3.Dot(ref this.normal, ref difference, out this.depth);
				this.depth = -this.depth;

				this.satisfied = this.velocityChange <= MathExtension.Epsilon;
			}
			else
			{
				this.normal = Vector3.Zero;
				this.satisfied = true;
			}
#else
			// Get the oriented relative position of the fixed point to each rigid body.
			this.position0 = Vector3.TransformNormal(this.RelativePosition0, this.rigidBody0.Orientation);
			this.position1 = Vector3.TransformNormal(this.RelativePosition1, this.rigidBody1.Orientation);

			// Get the world positions of the relative positions to the fixed point of each rigid body.
			Vector3 worldPosition0 = this.rigidBody0.Position + this.position0;
			Vector3 worldPosition1 = this.rigidBody1.Position + this.position1;

			// Get the vector between the two world positions.
			Vector3 difference = worldPosition0 - worldPosition1;
			if (difference.LengthSquared() > MathExtension.Epsilon * MathExtension.Epsilon)
			{
				// Unitize the differenct in position to get the correction
				// normal.
				this.normal = Vector3Extension.SafeNormalize(difference);
								
				// Determine the magnitude of an impulse necessary to remove all relative velocity at the relative fixed points of
				// each rigid body and combine them.
				this.velocityChange = 0.0f;
				if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					this.velocityChange = 
						this.rigidBody0.inverseMass + 
						Vector3.Dot(this.normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(this.position0, this.normal), this.rigidBody0.inverseInertiaTensorWorld), this.position0));
				}
				if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					this.velocityChange += 
						this.rigidBody1.inverseMass + 
						Vector3.Dot(this.normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(this.position1, this.normal), this.rigidBody1.inverseInertiaTensorWorld), this.position1));
				}

				// Get the error projected onto the normal.
				this.depth = -Vector3.Dot(this.normal, difference);

				this.satisfied = this.velocityChange <= MathExtension.Epsilon;
			}
			else
			{
				this.normal = Vector3.Zero;
				this.satisfied = true;
			}
#endif
#endif
		}

		internal protected override bool Solve(float elapsed)
		{
			// Assume the constraint has not been broken.
			this.satisfied = true;

			// Exit if both rigid bodies are disabled.
			if ((this.rigidBody0.Flags & this.rigidBody1.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.Disabled)
			{
				return false;
			}
#if INLINE
            // Get the relative velocity at the fixed point of the each rigid body.
			float x, y, z;
			x = y = z = 0.0f;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
				x = this.rigidBody0.Velocity.X + ((this.rigidBody0.Rotation.Y * this.position0.Z) - (this.rigidBody0.Rotation.Z * this.position0.Y));
				y = this.rigidBody0.Velocity.Y + ((this.rigidBody0.Rotation.Z * this.position0.X) - (this.rigidBody0.Rotation.X * this.position0.Z));
				z = this.rigidBody0.Velocity.Z + ((this.rigidBody0.Rotation.X * this.position0.Y) - (this.rigidBody0.Rotation.Y * this.position0.X));
			}
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
                x -= this.rigidBody1.Velocity.X + ((this.rigidBody1.Rotation.Y * this.position1.Z) - (this.rigidBody1.Rotation.Z * this.position1.Y));
                y -= this.rigidBody1.Velocity.Y + ((this.rigidBody1.Rotation.Z * this.position1.X) - (this.rigidBody1.Rotation.X * this.position1.Z));
                z -= this.rigidBody1.Velocity.Z + ((this.rigidBody1.Rotation.X * this.position1.Y) - (this.rigidBody1.Rotation.Y * this.position1.X));
            }

			// Determine the velocity magnitude in the direction of the combined relative velocity.
			float normalVelocity = this.normal.X * x + this.normal.Y * y + this.normal.Z * z;
#elif REFERENCE
            // Get the relative velocity at the fixed point of the each rigid body.
            Vector3 relativeVelocity = Vector3.Zero;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
				Vector3.Cross(ref this.rigidBody0.Rotation, ref this.position0, out relativeVelocity);
				Vector3.Add(ref relativeVelocity, ref this.rigidBody0.Velocity, out relativeVelocity);
			}
            if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
            {
				Vector3 temp;
                Vector3.Cross(ref this.rigidBody1.Rotation, ref this.position1, out temp);
                Vector3.Add(ref temp, ref this.rigidBody1.Velocity, out temp);
				Vector3.Subtract(ref relativeVelocity, ref temp, out relativeVelocity);
            }

            // Determine the velocity magnitude in the direction of the combined relative velocity.
			float normalVelocity;
			Vector3.Dot(ref this.normal, ref relativeVelocity, out normalVelocity);
#else
			// Get the relative velocity at the fixed point of the two rigid bodies.
			Vector3 relativeVelocity = Vector3.Zero;
			if ((this.rigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				relativeVelocity = this.rigidBody0.Velocity + Vector3.Cross(this.rigidBody0.Rotation, this.position0);
			}
			if ((this.rigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				relativeVelocity -= this.rigidBody1.Velocity + Vector3.Cross(this.rigidBody1.Rotation, this.position1);
			}

			// Determine the velocity magnitude in the direction of the combined relative velocity.
			float normalVelocity = Vector3.Dot(this.normal, relativeVelocity);
			// TODO : Maybe check for minimum velocity in the direction of normal.
#endif
			if (this.velocityChange > MathExtension.Epsilon)
			{
				const float tau = 0.3f;
				const float damping = 1.0f;

				// TODO : Do I need to apply elapsed to normalVelocity as well???
				// Determine the vector impulse necessary to move each rigid body closer to making this
				// constraint satisfied.
				float impulse = (this.depth * tau / elapsed - normalVelocity * damping) / this.velocityChange;
#if INLINE
				Vector3 normalImpulse = new Vector3()
				{
					X = this.normal.X * impulse,
					Y = this.normal.Y * impulse,
					Z = this.normal.Z * impulse,
				};
#elif REFERENCE
				Vector3 normalImpulse;
				Vector3.Multiply(ref this.normal, impulse, out normalImpulse);
#else
				Vector3 normalImpulse = this.normal * impulse;
#endif
				// Apply the impulse to each rigid body.
				this.rigidBody0.ApplyOffsetForceImpulse(ref normalImpulse, ref this.position0);
				this.rigidBody1.ApplyReverseOffsetForceImpulse(ref normalImpulse, ref this.position1);

				// Reset all collisions and constraints - a velocity change occurred.
				this.rigidBody0.CollisionGroup.Collisions.Reset();
				this.rigidBody0.Constraints.Reset();
				this.rigidBody1.CollisionGroup.Collisions.Reset();
				this.rigidBody1.Constraints.Reset();

				// Keep this constraint satisfied.
				this.satisfied = true;

				// Some more processing may be necessary.
				return true;
			}

			// The constraint does not need further processing.
			return false;
		}

		#endregion
	}
#endif
}
